#include "collisiondetector.h"
#include "intersectiontests.h"

#include <cassert>

using namespace TorPhysix;


unsigned CollisionDetector::sphereAndHalfSpace(const CollisionSphere &sphere, const CollisionPlane &plane, CollisionData *data)
{
	// Make sure we have contacts
	if (data->contactsLeft <= 0) return 0;

	// Cache the sphere position
	Vec3f position = sphere.getAxis(3);

	// Find the distance from the plane
	real ballDistance = plane.direction.dot(position) - sphere.radius - plane.offset;

	if (ballDistance >= 0) return 0;

	// Create the contact - it has a normal in the plane direction.
	Contact* contact = data->contacts;
	contact->contactNormal = plane.direction;
	contact->penetration = -ballDistance;
	contact->contactPoint = position - plane.direction * (ballDistance + sphere.radius);
	contact->setBodyData(sphere.body, NULL, data->friction, data->restitution);

	data->addContacts(1);
	return 1;
}


unsigned int CollisionDetector::sphereAndSphere(const CollisionSphere &one, const CollisionSphere &two, CollisionData *data)
{
	if( data->contactsLeft <= 0 ) return 0;

	Vec3f pos1 = one.getAxis(3);
	Vec3f pos2 = two.getAxis(3);

	Vec3f midline = pos1 - pos2;
	real size = length(midline);

	if( size <= 0.0f || size >= one.radius + two.radius )
	{
		return 0;
	}

	Vec3f normal = midline * ( (real) 1.0 / size);

	Contact* contact = data->contacts;
	contact->contactNormal = normal;
	contact->contactPoint = pos1 + midline * (real)0.5;
	contact->penetration = (one.radius + two.radius - size);
	contact->setBodyData(one.body, two.body, data->friction, data->restitution);
	
	data->addContacts(1);
	return 1;
}


unsigned TorPhysix::CollisionDetector::boxAndHalfSpace( const CollisionBox &box, const CollisionPlane &plane, CollisionData *data )
{
	// Make sure we have contacts
	if (data->contactsLeft <= 0) return 0;

	// Check for intersection
	if (!IntersectionTests::boxAndHalfSpace(box, plane))
	{
		return 0;
	}

	// We have an intersection, so find the intersection points. We can make
	// do with only checking vertices. If the box is resting on a plane
	// or on an edge, it will be reported as four or two contact points.

	// Go through each combination of + and - for each half-size
	static real mults[8][3] = {{1,1,1},{-1,1,1},{1,-1,1},{-1,-1,1},
							   {1,1,-1},{-1,1,-1},{1,-1,-1},{-1,-1,-1}};

	Contact* contact = data->contacts;
	unsigned contactsUsed = 0;
	for (unsigned i = 0; i < 8; i++) {

		// Calculate the position of each vertex
		Vec3f vertexPos(mults[i][0], mults[i][1], mults[i][2]);
		vertexPos.x() *= box.halfSize.x();
		vertexPos.y() *= box.halfSize.y();
		vertexPos.z() *= box.halfSize.z();
		vertexPos = transform(box.transform,vertexPos);

		// Calculate the distance from the plane
		real vertexDistance = vertexPos.dot( plane.direction );

		// Compare this to the plane's distance
		if (vertexDistance <= plane.offset)
		{
			// Create the contact data.

			// The contact point is halfway between the vertex and the
			// plane - we multiply the direction by half the separation
			// distance and add the vertex location.
			contact->contactPoint = plane.direction;
			contact->contactPoint *= (vertexDistance-plane.offset);
			contact->contactPoint += vertexPos;
			contact->contactNormal = plane.direction;
			contact->penetration = plane.offset - vertexDistance;

			// Write the appropriate data
			contact->setBodyData(box.body, NULL, data->friction, data->restitution);

			// Move onto the next contact
			contact++;
			contactsUsed++;
			if (contactsUsed == data->contactsLeft) return contactsUsed;
		}
	}

	data->addContacts(contactsUsed);
	return contactsUsed;
}

unsigned TorPhysix::CollisionDetector::boxAndSphere( const CollisionBox &box, const CollisionSphere &sphere, CollisionData *data )
{
	// Transform the centre of the sphere into box coordinates
	Vec3f centre = sphere.getAxis(3);
	// TODO add a transformInverse(Mat4f, Vec3f);
	Mat4f inverseBoxTform = inverse(box.transform);
	Vec3f relCentre = transform(inverseBoxTform, centre );

	// Early out check to see if we can exclude the contact
	if (real_abs(relCentre.x()) - sphere.radius > box.halfSize.x() ||
		real_abs(relCentre.y() ) - sphere.radius > box.halfSize.y() ||
		real_abs(relCentre.z() ) - sphere.radius > box.halfSize.z() )
	{
		return 0;
	}

	Vec3f closestPt(0.0f,0.0f,0.0f);
	real dist;

	// Clamp each coordinate to the box.
	dist = relCentre.x();
	if (dist > box.halfSize.x()) dist = box.halfSize.x();
	if (dist < -box.halfSize.x()) dist = -box.halfSize.x();
	closestPt.x() = dist;

	dist = relCentre.y();
	if (dist > box.halfSize.y()) dist = box.halfSize.y();
	if (dist < -box.halfSize.y()) dist = -box.halfSize.y();
	closestPt.y() = dist;

	dist = relCentre.z();
	if (dist > box.halfSize.z()) dist = box.halfSize.z();
	if (dist < -box.halfSize.z()) dist = -box.halfSize.z();
	closestPt.z() = dist;

	// Check we're in contact
	dist = length_2(closestPt - relCentre);
	if (dist > sphere.radius * sphere.radius) return 0;

	// Compile the contact
	Vec3f closestPtWorld = transform(box.transform, closestPt);
	if( closestPtWorld == centre ) throw "cannot create normal, objs are at same position ";
	Contact* contact = data->contacts;
	contact->contactNormal = (closestPtWorld - centre);
	contact->contactNormal = normalize(contact->contactNormal);
	contact->contactPoint = closestPtWorld;
	contact->penetration = sphere.radius - real_sqrt(dist);

	contact->setBodyData(box.body , sphere.body, data->friction, data->restitution);
	//contact->setBodyData(NULL, sphere.body, data->friction, data->restitution);

	data->addContacts(1);
	return 1;
}
#pragma region BOXANDBOX

static inline real transformToAxis(const CollisionBox &box,const Vec3f &axis)
{
	return
		box.halfSize.x() * real_abs(axis.dot( box.getAxis(0)) )+
		box.halfSize.y() * real_abs(axis.dot( box.getAxis(1)) )+
		box.halfSize.z() * real_abs(axis.dot( box.getAxis(2)) );
}

/*
 * This function checks if the two boxes overlap
 * along the given axis, returning the amount of overlap.
 * The final parameter toCentre
 * is used to pass in the vector between the boxes centre
 * points, to avoid having to recalculate it each time.
 */
static inline real penetrationOnAxis(const CollisionBox &one, const CollisionBox &two, const Vec3f &axis, const Vec3f &toCentre)
{
    // Project the half-size of one onto axis
    real oneProject = transformToAxis(one, axis);
    real twoProject = transformToAxis(two, axis);

    // Project this onto the axis
    real distance = real_abs(toCentre.dot(axis) );

    // Return the overlap (i.e. positive indicates
    // overlap, negative indicates separation).
    return oneProject + twoProject - distance;
}


static inline bool tryAxis(const CollisionBox &one, const CollisionBox &two, Vec3f& axis, const Vec3f& toCentre, unsigned index,
							// These values may be updated
							real& smallestPenetration,
							unsigned &smallestCase
							)
{
    // Make sure we have a normalized axis, and don't check almost parallel axes
    if ( length_2(axis) < 0.0001) return true;
    axis = normalize(axis);

    real penetration = penetrationOnAxis(one, two, axis, toCentre);

    if (penetration < 0) return false;
    if (penetration < smallestPenetration) {
        smallestPenetration = penetration;
        smallestCase = index;
    }
    return true;
}

static inline bool tryAxis(const CollisionBox &one, const CollisionBox &two, Vec3f axis, const Vec3f& toCentre, unsigned index,
						   // These values may be updated
						   real& smallestPenetration,
						   unsigned &smallestCase
						   )
{
	// Make sure we have a normalized axis, and don't check almost parallel axes
	if ( length_2(axis) < 0.0001) return true;
	axis = normalize(axis);

	real penetration = penetrationOnAxis(one, two, axis, toCentre);

	if (penetration < 0) return false;
	if (penetration < smallestPenetration) {
		smallestPenetration = penetration;
		smallestCase = index;
	}
	return true;
}

void fillPointFaceBoxBox(const CollisionBox &one, const CollisionBox &two, const Vec3f &toCentre, CollisionData *data, unsigned best, real pen )
{
    // This method is called when we know that a vertex from
    // box two is in contact with box one.

    Contact* contact = data->contacts;

    // We know which axis the collision is on (i.e. best),
    // but we need to work out which of the two faces on
    // this axis.
    Vec3f normal = one.getAxis(best);
    if (one.getAxis(best).dot(toCentre) > 0)
    {
        normal = normal * -1.0f;
    }

    // Work out which vertex of box two we're colliding with.
    // Using toCentre doesn't work!
    Vec3f vertex = two.halfSize;
    if (two.getAxis(0).dot( normal ) < 0) vertex.x() = -vertex.x();
    if (two.getAxis(1).dot( normal ) < 0) vertex.y() = -vertex.y();
    if (two.getAxis(2).dot( normal ) < 0) vertex.z() = -vertex.z();

    // Create the contact data
    contact->contactNormal = normal;
    contact->penetration = pen;
    contact->contactPoint = transform( two.getTransform(),  vertex);
    contact->setBodyData(one.body, two.body, data->friction, data->restitution);
}

static inline Vec3f contactPoint(const Vec3f &pOne, const Vec3f &dOne, real oneSize,
								 const Vec3f &pTwo, const Vec3f &dTwo, real twoSize,
								// If this is true, and the contact point is outside
								// the edge (in the case of an edge-face contact) then
								// we use one's midpoint, otherwise we use two's.
								bool useOne)
{
    Vec3f toSt, cOne, cTwo;
    real dpStaOne, dpStaTwo, dpOneTwo, smOne, smTwo;
    real denom, mua, mub;

    smOne = length_2(dOne);
    smTwo = length_2(dTwo);
    dpOneTwo = dTwo.dot(dOne);

    toSt = pOne - pTwo;
    dpStaOne = dOne.dot(toSt);
    dpStaTwo = dTwo.dot(toSt);

    denom = smOne * smTwo - dpOneTwo * dpOneTwo;

    // Zero denominator indicates parallel lines
    if (real_abs(denom) < 0.0001f) {
        return useOne?pOne:pTwo;
    }

    mua = (dpOneTwo * dpStaTwo - smTwo * dpStaOne) / denom;
    mub = (smOne * dpStaTwo - dpOneTwo * dpStaOne) / denom;

    // If either of the edges has the nearest point out
    // of bounds, then the edges aren't crossed, we have
    // an edge-face contact. Our point is on the edge, which
    // we know from the useOne parameter.
    if (mua > oneSize ||
        mua < -oneSize ||
        mub > twoSize ||
        mub < -twoSize)
    {
        return useOne?pOne:pTwo;
    }
    else
    {
        cOne = pOne + dOne * mua;
        cTwo = pTwo + dTwo * mub;

        return cOne * 0.5 + cTwo * 0.5;
    }
}

// This preprocessor definition is only used as a convenience
// in the boxAndBox contact generation method.
#define CHECK_OVERLAP(axis, index) \
    if (!tryAxis(one, two, (axis), toCentre, (index), pen, best)) return 0;

unsigned CollisionDetector::boxAndBox( const CollisionBox &one, const CollisionBox &two, CollisionData *data )
{
    //if (!IntersectionTests::boxAndBox(one, two)) return 0;

    // Find the vector between the two centers
    Vec3f toCentre = two.getAxis(3) - one.getAxis(3);

    // We start assuming there is no contact
    real pen = REAL_MAX;
    unsigned best = 0xffffff;

    // Now we check each axes, returning if it gives us
    // a separating axis, and keeping track of the axis with
    // the smallest penetration otherwise.
    CHECK_OVERLAP(one.getAxis(0), 0);
    CHECK_OVERLAP(one.getAxis(1), 1);
    CHECK_OVERLAP(one.getAxis(2), 2);

    CHECK_OVERLAP(two.getAxis(0), 3);
    CHECK_OVERLAP(two.getAxis(1), 4);
    CHECK_OVERLAP(two.getAxis(2), 5);

    // Store the best axis-major, in case we run into almost
    // parallel edge collisions later
    unsigned bestSingleAxis = best;

    CHECK_OVERLAP(one.getAxis(0).cross( two.getAxis(0) ), 6);
    CHECK_OVERLAP(one.getAxis(0).cross( two.getAxis(1) ), 7);
    CHECK_OVERLAP(one.getAxis(0).cross( two.getAxis(2) ), 8);
    CHECK_OVERLAP(one.getAxis(1).cross( two.getAxis(0) ), 9);
    CHECK_OVERLAP(one.getAxis(1).cross( two.getAxis(1) ), 10);
    CHECK_OVERLAP(one.getAxis(1).cross( two.getAxis(2) ), 11);
    CHECK_OVERLAP(one.getAxis(2).cross( two.getAxis(0) ), 12);
    CHECK_OVERLAP(one.getAxis(2).cross( two.getAxis(1) ), 13);
    CHECK_OVERLAP(one.getAxis(2).cross( two.getAxis(2) ), 14);

    // Make sure we've got a result.
    //assert(best != 0xffffff);
	if( best == 0xffffff)
	{
		printf("un-handled box-vs-box case\n");
		throw "un-handled box-vs-box case";
		return 0;
	}

    // We now know there's a collision, and we know which
    // of the axes gave the smallest penetration. We now
    // can deal with it in different ways depending on
    // the case.
    if (best < 3)
    {
        // We've got a vertex of box two on a face of box one.
        fillPointFaceBoxBox(one, two, toCentre, data, best, pen);
        data->addContacts(1);
        return 1;
    }
    else if (best < 6)
    {
        // We've got a vertex of box one on a face of box two.
        // We use the same algorithm as above, but swap around
        // one and two (and therefore also the vector between their
        // centers).
        fillPointFaceBoxBox(two, one, toCentre*-1.0f, data, best-3, pen);
        data->addContacts(1);
        return 1;
    }
    else
    {
#pragma region EDGE_VS_EDGE
        // We've got an edge-edge contact. Find out which axes
        best -= 6;
        unsigned oneAxisIndex = best / 3;
        unsigned twoAxisIndex = best % 3;
        Vec3f oneAxis = one.getAxis(oneAxisIndex);
        Vec3f twoAxis = two.getAxis(twoAxisIndex);
        Vec3f axis = oneAxis.cross( twoAxis );
        axis = normalize(axis);

        // The axis should point from box one to box two.
        if (axis.dot(toCentre) > 0) axis = axis * -1.0f;

        // We have the axes, but not the edges: each axis has 4 edges parallel
        // to it, we need to find which of the 4 for each object. We do
        // that by finding the point in the centre of the edge. We know
        // its component in the direction of the box's collision axis is zero
        // (its a mid-point) and we determine which of the extremes in each
        // of the other axes is closest.
        Vec3f ptOnOneEdge = one.halfSize;
        Vec3f ptOnTwoEdge = two.halfSize;
        for (unsigned i = 0; i < 3; i++)
        {
            if (i == oneAxisIndex) ptOnOneEdge[i] = 0;
            else if (one.getAxis(i).dot(axis) > 0) ptOnOneEdge[i] = -ptOnOneEdge[i];

            if (i == twoAxisIndex) ptOnTwoEdge[i] = 0;
            else if (two.getAxis(i).dot(axis) < 0) ptOnTwoEdge[i] = -ptOnTwoEdge[i];
        }

        // Move them into world coordinates (they are already oriented
        // correctly, since they have been derived from the axes).
        ptOnOneEdge = transform(one.getTransform(), ptOnOneEdge);
        ptOnTwoEdge = transform(two.getTransform(), ptOnTwoEdge);

        // So we have a point and a direction for the colliding edges.
        // We need to find out point of closest approach of the two
        // line-segments.
        Vec3f vertex = contactPoint(ptOnOneEdge, oneAxis, one.halfSize[oneAxisIndex], ptOnTwoEdge, twoAxis, two.halfSize[twoAxisIndex], bestSingleAxis > 2 );

        // We can fill the contact.
        Contact* contact = data->contacts;

        contact->penetration = pen;
        contact->contactNormal = axis;
        contact->contactPoint = vertex;
        contact->setBodyData(one.body, two.body, data->friction, data->restitution);
        data->addContacts(1);
        return 1;
#pragma endregion EDGE_VS_EDGE
	}
	
	//return 0; // unreachable
}
#undef CHECK_OVERLAP

#pragma endregion BOXANDBOX
